#pragma once
#include <constants.h>
#include <CGAL_Utils.hpp>
#include <PlaneParas.hpp>
#include <PlaneUtils.hpp>

using namespace std;

/*****************************************
 * 这个类维护着全局平面形成的地图
 * 主要作用:
 * - 将单帧平面变换到全局坐标系
 * - 若有重复平面，只更新不添加，并记录更新的平面索引
 *****************************************/

class PlaneMapper{
public:
    PlaneMapper():
		pu(new PlaneUtils()){}

    ~PlaneMapper(){}

	void addPlane(const Plane& p)
	{
		switch(p.getLabel())
		{
			case CEILING: ceiling_plane_map.push_back(p); break;
			case FLOOR: floor_plane_map.push_back(p); break;
			case WALL: wall_plane_map.push_back(p); break;
		}
	}
	
	void processPlane(Plane& plane, Matrix4d& t)
	{
		if(!inited) // 平面地图初始化
		{
			plane.transform(t);
			addPlane(plane);
			return;
		}
		else
		{
			switch(plane.getLabel())
			{
				case CEILING: processPlane(ceiling_plane_map, plane, t); break;
				case FLOOR: processPlane(floor_plane_map, plane, t); break;
			}
		}
	}

    void processWallPlanes(vector<Plane> &planes, Matrix4d &t,vector<int>& update_plane_idx, vector<int>& new_plane_idx)
    {
		if(!inited) // 平面地图初始化
		{
			for(auto &p : planes)
			{
				p.transform(t);
				addPlane(p);
			}
			inited = true;
			return;
		}
		
		for(auto &p : planes)
			processWallPlane(p, t, update_plane_idx, new_plane_idx);
    }

	void processPlane(vector<Plane>& plane_map, Plane& plane, Matrix4d& t)
	{
		vector<PlaneParas> local_paras;
		pu->getPlaneParasSeenFromLocalCamera(plane_map, t, local_paras);

		// 找的时候从最新添加的平面往回找，效率更高一些
		int candidate_idx = pu->findMatchPlane(PlaneParas(plane), local_paras);
		

		std::cout << candidate_idx << std::endl;
		// plane_map_seen_from_local = pu->getPlanesSeenFromLocalCamera(plane_map, t);
		// int candidate_idx = pu->findMatchPlane(plane, plane_map_seen_from_local);
		std::cout << "Plane: " << plane.getLabel() << ", size: " << plane.getPointCloud()->points.size() << std::endl;
		
		// 变换到全局坐标系下
		//Plane plane_transformed = plane.getPlaneSeenFromGlobalCamera(t);
		plane.transform(t);

		if(candidate_idx == -1) // 是一个新的平面
		{	
			// 打印这个平面被判定为新平面时的阈值
			// pu->findMatchPlane(PlaneParas(plane), local_paras, true);
			// pu->findMatchPlane(plane, plane_map_seen_from_local, true);
			
			// 添加平面
			addPlane(plane);
		}
		else if (candidate_idx == -2) // 是一个估计出错的平面，应该舍弃它
		{
			return;
		}
		else // 地图中已存在一个非常相近的平面
		{	// 合并就行，不新增平面
			//std::cout << plane_map[candidate_idx].getPoints().size() << std::endl; 
			pu->mergePlanes(plane_map[candidate_idx], plane);
			//std::cout << plane_map[candidate_idx].getPoints().size() << std::endl;
		}
	}

	// 只记录墙面的更新，默认Framework中的地面和天花板已经固定
    void processWallPlane(Plane &plane, Matrix4d& t, vector<int>& update_plane_idx, vector<int>& new_plane_idx)
    {
		// std::cout << "processing wall..." << std::endl;
		vector<PlaneParas> local_paras;

		//PlaneParas forprint(wall_plane_map[0]);
		//std::cout << "origin:\n" << forprint.normal << std::endl;
		pu->getPlaneParasSeenFromLocalCamera(wall_plane_map, t, local_paras);
		//std::cout << "transformed:\n" << local_paras[0].normal << std::endl;

		// 找的时候从最新添加的平面往回找，效率更高一些
		int candidate_idx = pu->findMatchPlane(PlaneParas(plane), local_paras);
		

		//Plane plane_transformed = plane.getPlaneSeenFromGlobalCamera(t);
		plane.transform(t);

		if(candidate_idx == -1) // 是一个新的平面
		{	
			// 打印这个平面被判定为新平面时的阈值
			//pu->findMatchPlane(plane, plane_map_seen_from_local, true);
			//pu->findMatchPlane(PlaneParas(plane), local_paras, true);
			
			// 添加平面
			addPlane(plane);
			new_plane_idx.push_back(wall_plane_map.size()-1);
		}
		else if (candidate_idx == -2) // 是一个估计出错的平面，应该舍弃它
		{
			return;
		}
		else // 地图中已存在一个非常相近的平面
		{	// 合并就行，不新增平面
			pu->mergePlanes(wall_plane_map[candidate_idx], plane);
			
			// 计算新增点云的bbox给框架模块
			plane.computeAABB();

			wall_plane_map[candidate_idx].setAABB(plane.getAABB());

			// 记录发生改变的平面索引
			update_plane_idx.push_back(candidate_idx);
		}
    }

	const vector<Plane>& getCeilingPlanes() { return ceiling_plane_map;}
	const vector<Plane>& getFloorPlanes() { return floor_plane_map;}
	const vector<Plane>& getWallPlanes() { return wall_plane_map;}

	void setOutDir(string value) { out_dir = value;}

private:
	bool inited = false;
	PlaneUtils *pu;
	
	vector<Plane> ceiling_plane_map;
	vector<Plane> floor_plane_map;
	vector<Plane> wall_plane_map;
	
	vector<Plane> plane_map_seen_from_local;	
	PointCloud::Ptr plane_map_cloud;
	string out_dir;
};
